// #pragma once

// #include <bsp.h>
// /* --------------------------- */
// #include <algorithm/math.h>
// #include <sheriff_os.h>
// #

// // ------ spi driver ------
// namespace bsp {
// class SpiDevice {
// private:
//   SPI_HandleTypeDef* _hspi;

// public:
//   /**
//    * @brief New a SpiDevice
//    * @param hspi Spi handler
//    * @author CharlesHsu
//    */
//   explicit SpiDevice(SPI_HandleTypeDef* hspi);

//   /**
//    * @brief Init the spi device
//    * @param config [SPI_InitTypeDef] The default value is {BaudRatePrescaler :
//    * SPI_BAUDRATEPRESCALER_8}
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* init(SPI_InitTypeDef config = {
//       .BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8});

//   /**
//    * @brief receive spi data (for DMA)
//    * @param buffer Buffer pointer
//    * @param len Buffer length
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* receiveDataDMA(uint8_t* buffer, uint16_t len);

//   /**
//    * @brief receive spi data
//    * @param buffer Buffer pointer
//    * @param len Buffer length
//    * @param timeout [ms] Timeout duration. The default value is 10 ms
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* receiveData(uint8_t* buffer, uint16_t len, uint32_t timeout = 10);

//   /**
//    * @brief Send spi data (for DMA)
//    * @param buffer Buffer pointer
//    * @param len Buffer length
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* sendDataDMA(uint8_t* buffer, uint16_t len);

//   /**
//    * @brief Send spi data
//    * @param buffer Buffer pointer
//    * @param len Buffer length
//    * @param timeout [ms] Timeout duration. The default value is 10 ms
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* sendData(uint8_t* buffer, uint16_t len, uint32_t timeout = 10);

//   /**
//    * @brief Swap one byte data or command to spi device
//    * @param tx_data data to be sent
//    * @param rx_data_ptr pointer to reception data buffer
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* swapOneByteData(uint8_t tx_data, uint8_t* rx_data_ptr);

//   /**
//    * @brief Swap multi data or command to spi address
//    * @param command Data to transmit
//    * @param rx_data_ptr Pointer to reception data buffer
//    * @param length Length of reception data buffer
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* readMultiReg(uint8_t command, uint8_t* rx_data_ptr,
//                           uint8_t length);

//   /**
//    * @brief Swap data or command to spi address (for DMA)
//    * @param tx_data_ptr Pointer to the transmission data buffer
//    * @param rx_data_ptr Pointer to reception data buffer
//    * @param len Length of data buffer
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr,
//                          uint16_t len);

//   /**
//    * @brief Swap data or command to spi address
//    * @param tx_data_ptr Pointer to the transmission data buffer
//    * @param rx_data_ptr Pointer to reception data buffer
//    * @param len Length of data buffer
//    * @param timeout [ms] Timeout duration. The default value is 10 ms
//    * @author CharlesHsu
//    * @return this
//    */
//   SpiDevice* swapDataDMA(uint8_t* tx_data_ptr, uint8_t* rx_data_ptr,
//                          uint16_t len, uint32_t timeout = 10);

//   ~SpiDevice() = default;
// };
// } // namespace bsp

// namespace bsp::spi::sd {
// void WriteByte(SPI_HandleTypeDef* h_spi, uint8_t data, uint32_t time_out);

// void WriteBuffer(SPI_HandleTypeDef* h_spi, uint8_t* buffer, uint16_t len,
//                  uint32_t time_out);

// uint8_t ReadByte(SPI_HandleTypeDef* h_spi, uint32_t time_out);
// } // namespace bsp::spi::sd

// namespace bsp::spi::bmi {
// void SPI_Init(SPI_HandleTypeDef* hspi);

// void SPI_Send(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len,
//               uint32_t timeout);

// void SPI_Receive(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len,
//                  uint32_t timeout);

// void SPI_SendDMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len);

// void SPI_ReceiveDMA(SPI_HandleTypeDef* hspi, uint8_t* pData, uint16_t len);

// void SPI_Swap(SPI_HandleTypeDef* hspi, uint8_t* pTxData, uint8_t* pRxData,
//               uint16_t len, uint32_t timeout);

// void SPI_SwapDMA(SPI_HandleTypeDef* hspi, uint8_t* pTxData, uint8_t* pRxData,
//                  uint16_t len);

// uint8_t SPI_SwapAbyte(SPI_HandleTypeDef* hspi, uint8_t txdata);

// void SPI_ReadMuliReg(SPI_HandleTypeDef* hspi, uint8_t* rx_data, uint8_t len);
// } // namespace bsp::spi::bmi

// namespace bsp {
// class GpioDriver {
// private:
//   GPIO_TypeDef* _gpio;
//   uint16_t _pin;
//   GPIO_PinState _state;

// public:
//   GpioDriver(GPIO_TypeDef* gpio, uint16_t pin,
//              GPIO_PinState state = GPIO_PIN_RESET);

//   GpioDriver* reset();

//   GpioDriver* set();

//   GpioDriver* toggle();

//   GPIO_PinState read();

//   ~GpioDriver() = default;
// };
// } // namespace bsp

// constexpr auto GRAVITY =
//     units::acceleration::meters_per_second_squared_t(9.81f);

// constexpr auto BMI088_COM_WAIT_SENSOR_TIME = 2;
// constexpr auto BMI088_LONG_DELAY_TIME = 65;

// using AccelStruct = struct {
//   units::acceleration::meters_per_second_squared_t x;
//   units::acceleration::meters_per_second_squared_t y;
//   units::acceleration::meters_per_second_squared_t z;
// };
// using GyroStruct = struct {
//   units::angular_velocity::degrees_per_second_t yaw;
//   units::angular_velocity::degrees_per_second_t pitch;
//   units::angular_velocity::degrees_per_second_t roll;
// };
// using ConsequentAngle = struct {
//   units::angle::degree_t limitedAngle;
//   units::dimensionless::scalar_t rounds;
// };

// enum {
//   BMI088_NO_ERROR = 0x00,
//   BMI088_ACC_PWR_CTRL_ERROR = 0x01,
//   BMI088_ACC_PWR_CONF_ERROR = 0x02,
//   BMI088_ACC_CONF_ERROR = 0x03,
//   BMI088_ACC_SELF_TEST_ERROR = 0x04,
//   BMI088_ACC_RANGE_ERROR = 0x05,
//   BMI088_INT1_IO_CTRL_ERROR = 0x06,
//   BMI088_INT_MAP_DATA_ERROR = 0x07,
//   BMI088_GYRO_RANGE_ERROR = 0x08,
//   BMI088_GYRO_BANDWIDTH_ERROR = 0x09,
//   BMI088_GYRO_LPM1_ERROR = 0x0A,
//   BMI088_GYRO_CTRL_ERROR = 0x0B,
//   BMI088_GYRO_INT3_INT4_IO_CONF_ERROR = 0x0C,
//   BMI088_GYRO_INT3_INT4_IO_MAP_ERROR = 0x0D,

//   BMI088_SELF_TEST_ACCEL_ERROR = 0x80,
//   BMI088_SELF_TEST_GYRO_ERROR = 0x40,
//   BMI088_NO_SENSOR = 0xFF,
// };

// #define BMI088_BOARD_INSTALL_SPIN_MATRIX \
//   {0.0f, 1.0f, 0.0f}, {-1.0f, 0.0f, 0.0f}, { 0.0f, 0.0f, 1.0f }

// #define BMI088_TEMP_FACTOR 0.125f
// #define BMI088_TEMP_OFFSET 23.0f

// #define BMI088_GxOFFSET 0.00247530174f
// #define BMI088_GyOFFSET 0.000393082853f
// #define BMI088_GzOFFSET 0.000393082853f
// #define BMI088_gNORM 9.69293118f

// #define BMI088_WRITE_ACCEL_REG_NUM 6
// #define BMI088_WRITE_GYRO_REG_NUM 6

// #define BMI088_GYRO_DATA_READY_BIT 0
// #define BMI088_ACCEL_DATA_READY_BIT 1
// #define BMI088_ACCEL_TEMP_DATA_READY_BIT 2

// #define BMI088_ACCEL_IIC_ADDRESSE (0x18 << 1)
// #define BMI088_GYRO_IIC_ADDRESSE (0x68 << 1)

// #define BMI088_ACCEL_RANGE_3G
// #define BMI088_ACCEL_RANGE_6G
// #define BMI088_ACCEL_RANGE_12G
// #define BMI088_ACCEL_RANGE_24G

// #define BMI088_GYRO_RANGE_2000
// #define BMI088_GYRO_RANGE_1000
// #define BMI088_GYRO_RANGE_500
// #define BMI088_GYRO_RANGE_250
// #define BMI088_GYRO_RANGE_125

// #define BMI088_ACCEL_3G_SEN 0.0008974358974f
// #define BMI088_ACCEL_6G_SEN 0.00179443359375f
// #define BMI088_ACCEL_12G_SEN 0.0035888671875f
// #define BMI088_ACCEL_24G_SEN 0.007177734375f

// #define BMI088_GYRO_2000_SEN 0.00106526443603169529841533860381f
// #define BMI088_GYRO_1000_SEN 0.00053263221801584764920766930190693f
// #define BMI088_GYRO_500_SEN 0.00026631610900792382460383465095346f
// #define BMI088_GYRO_250_SEN 0.00013315805450396191230191732547673f
// #define BMI088_GYRO_125_SEN 0.000066579027251980956150958662738366f
// #define BMI088_ACC_CHIP_ID 0x00  // the register is  " Who am I "
// #define BMI088_ACC_CHIP_ID_VALUE 0x1E
// // #define BMI088_ACC_CHIP_ID_VALUE 0xFA

// #define BMI088_ACC_ERR_REG 0x02
// #define BMI088_ACCEL_CONGIF_ERROR_SHFITS 0x2
// #define BMI088_ACCEL_CONGIF_ERROR (1 << BMI088_ACCEL_CONGIF_ERROR_SHFITS)
// #define BMI088_FATAL_ERROR_SHFITS 0x0
// #define BMI088_FATAL_ERROR (1 << BMI088_FATAL_ERROR)

// #define BMI088_ACC_STATUS 0x03
// #define BMI088_ACCEL_DRDY_SHFITS 0x7
// #define BMI088_ACCEL_DRDY (1 << BMI088_ACCEL_DRDY_SHFITS)

// #define BMI088_ACCEL_XOUT_L 0x12
// #define BMI088_ACCEL_XOUT_M 0x13
// #define BMI088_ACCEL_YOUT_L 0x14
// #define BMI088_ACCEL_YOUT_M 0x15
// #define BMI088_ACCEL_ZOUT_L 0x16
// #define BMI088_ACCEL_ZOUT_M 0x17

// #define BMI088_SENSORTIME_DATA_L 0x18
// #define BMI088_SENSORTIME_DATA_M 0x19
// #define BMI088_SENSORTIME_DATA_H 0x1A

// #define BMI088_ACC_INT_STAT_1 0x1D
// #define BMI088_ACCEL_DRDY_INTERRUPT_SHFITS 0x7
// #define BMI088_ACCEL_DRDY_INTERRUPT (1 << BMI088_ACCEL_DRDY_INTERRUPT_SHFITS)

// #define BMI088_TEMP_M 0x22
// #define BMI088_TEMP_L 0x23

// #define BMI088_ACC_CONF 0x40
// #define BMI088_ACC_CONF_MUST_Set 0x80
// #define BMI088_ACC_BWP_SHFITS 0x4
// #define BMI088_ACC_OSR4 (0x0 << BMI088_ACC_BWP_SHFITS)
// #define BMI088_ACC_OSR2 (0x1 << BMI088_ACC_BWP_SHFITS)
// #define BMI088_ACC_NORMAL (0x2 << BMI088_ACC_BWP_SHFITS)

// #define BMI088_ACC_ODR_SHFITS 0x0
// #define BMI088_ACC_12_5_HZ (0x5 << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_25_HZ (0x6 << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_50_HZ (0x7 << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_100_HZ (0x8 << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_200_HZ (0x9 << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_400_HZ (0xA << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_800_HZ (0xB << BMI088_ACC_ODR_SHFITS)
// #define BMI088_ACC_1600_HZ (0xC << BMI088_ACC_ODR_SHFITS)

// #define BMI088_ACC_RANGE 0x41
// #define BMI088_ACC_RANGE_SHFITS 0x0
// #define BMI088_ACC_RANGE_3G (0x0 << BMI088_ACC_RANGE_SHFITS)
// #define BMI088_ACC_RANGE_6G (0x1 << BMI088_ACC_RANGE_SHFITS)
// #define BMI088_ACC_RANGE_12G (0x2 << BMI088_ACC_RANGE_SHFITS)
// #define BMI088_ACC_RANGE_24G (0x3 << BMI088_ACC_RANGE_SHFITS)

// #define BMI088_INT1_IO_CTRL 0x53
// #define BMI088_ACC_INT1_IO_ENABLE_SHFITS 0x3
// #define BMI088_ACC_INT1_IO_ENABLE (0x1 << BMI088_ACC_INT1_IO_ENABLE_SHFITS)
// #define BMI088_ACC_INT1_GPIO_MODE_SHFITS 0x2
// #define BMI088_ACC_INT1_GPIO_PP (0x0 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
// #define BMI088_ACC_INT1_GPIO_OD (0x1 << BMI088_ACC_INT1_GPIO_MODE_SHFITS)
// #define BMI088_ACC_INT1_GPIO_LVL_SHFITS 0x1
// #define BMI088_ACC_INT1_GPIO_LOW (0x0 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)
// #define BMI088_ACC_INT1_GPIO_HIGH (0x1 << BMI088_ACC_INT1_GPIO_LVL_SHFITS)

// #define BMI088_INT2_IO_CTRL 0x54
// #define BMI088_ACC_INT2_IO_ENABLE_SHFITS 0x3
// #define BMI088_ACC_INT2_IO_ENABLE (0x1 << BMI088_ACC_INT2_IO_ENABLE_SHFITS)
// #define BMI088_ACC_INT2_GPIO_MODE_SHFITS 0x2
// #define BMI088_ACC_INT2_GPIO_PP (0x0 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
// #define BMI088_ACC_INT2_GPIO_OD (0x1 << BMI088_ACC_INT2_GPIO_MODE_SHFITS)
// #define BMI088_ACC_INT2_GPIO_LVL_SHFITS 0x1
// #define BMI088_ACC_INT2_GPIO_LOW (0x0 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)
// #define BMI088_ACC_INT2_GPIO_HIGH (0x1 << BMI088_ACC_INT2_GPIO_LVL_SHFITS)

// #define BMI088_INT_MAP_DATA 0x58
// #define BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS 0x6
// #define BMI088_ACC_INT2_DRDY_INTERRUPT \
//   (0x1 << BMI088_ACC_INT2_DRDY_INTERRUPT_SHFITS)
// #define BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS 0x2
// #define BMI088_ACC_INT1_DRDY_INTERRUPT \
//   (0x1 << BMI088_ACC_INT1_DRDY_INTERRUPT_SHFITS)

// #define BMI088_ACC_SELF_TEST 0x6D
// #define BMI088_ACC_SELF_TEST_OFF 0x00
// #define BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL 0x0D
// #define BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL 0x09

// #define BMI088_ACC_PWR_CONF 0x7C
// #define BMI088_ACC_PWR_SUSPEND_MODE 0x03
// #define BMI088_ACC_PWR_ACTIVE_MODE 0x00

// #define BMI088_ACC_PWR_CTRL 0x7D
// #define BMI088_ACC_ENABLE_ACC_OFF 0x00
// #define BMI088_ACC_ENABLE_ACC_ON 0x04

// #define BMI088_ACC_SOFTRESET 0x7E
// #define BMI088_ACC_SOFTRESET_VALUE 0xB6

// #define BMI088_GYRO_CHIP_ID 0x00
// #define BMI088_GYRO_CHIP_ID_VALUE 0x0F

// #define BMI088_GYRO_X_L 0x02
// #define BMI088_GYRO_X_H 0x03
// #define BMI088_GYRO_Y_L 0x04
// #define BMI088_GYRO_Y_H 0x05
// #define BMI088_GYRO_Z_L 0x06
// #define BMI088_GYRO_Z_H 0x07

// #define BMI088_GYRO_INT_STAT_1 0x0A
// #define BMI088_GYRO_DYDR_SHFITS 0x7
// #define BMI088_GYRO_DYDR (0x1 << BMI088_GYRO_DYDR_SHFITS)

// #define BMI088_GYRO_RANGE 0x0F
// #define BMI088_GYRO_RANGE_SHFITS 0x0
// #define BMI088_GYRO_2000 (0x0 << BMI088_GYRO_RANGE_SHFITS)
// #define BMI088_GYRO_1000 (0x1 << BMI088_GYRO_RANGE_SHFITS)
// #define BMI088_GYRO_500 (0x2 << BMI088_GYRO_RANGE_SHFITS)
// #define BMI088_GYRO_250 (0x3 << BMI088_GYRO_RANGE_SHFITS)
// #define BMI088_GYRO_125 (0x4 << BMI088_GYRO_RANGE_SHFITS)

// #define BMI088_GYRO_BANDWIDTH 0x10

// // the first num means Output data  rate, the second num means bandwidth
// #define BMI088_GYRO_BANDWIDTH_MUST_Set 0x80
// #define BMI088_GYRO_2000_532_HZ 0x00
// #define BMI088_GYRO_2000_230_HZ 0x01
// #define BMI088_GYRO_1000_116_HZ 0x02
// #define BMI088_GYRO_400_47_HZ 0x03
// #define BMI088_GYRO_200_23_HZ 0x04
// #define BMI088_GYRO_100_12_HZ 0x05
// #define BMI088_GYRO_200_64_HZ 0x06
// #define BMI088_GYRO_100_32_HZ 0x07

// #define BMI088_GYRO_LPM1 0x11
// #define BMI088_GYRO_NORMAL_MODE 0x00
// #define BMI088_GYRO_SUSPEND_MODE 0x80
// #define BMI088_GYRO_DEEP_SUSPEND_MODE 0x20

// #define BMI088_GYRO_SOFTRESET 0x14
// #define BMI088_GYRO_SOFTRESET_VALUE 0xB6

// #define BMI088_GYRO_CTRL 0x15
// #define BMI088_DRDY_OFF 0x00
// #define BMI088_DRDY_ON 0x80

// #define BMI088_GYRO_INT3_INT4_IO_CONF 0x16
// #define BMI088_GYRO_INT4_GPIO_MODE_SHFITS 0x3
// #define BMI088_GYRO_INT4_GPIO_PP (0x0 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
// #define BMI088_GYRO_INT4_GPIO_OD (0x1 << BMI088_GYRO_INT4_GPIO_MODE_SHFITS)
// #define BMI088_GYRO_INT4_GPIO_LVL_SHFITS 0x2
// #define BMI088_GYRO_INT4_GPIO_LOW (0x0 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
// #define BMI088_GYRO_INT4_GPIO_HIGH (0x1 << BMI088_GYRO_INT4_GPIO_LVL_SHFITS)
// #define BMI088_GYRO_INT3_GPIO_MODE_SHFITS 0x1
// #define BMI088_GYRO_INT3_GPIO_PP (0x0 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
// #define BMI088_GYRO_INT3_GPIO_OD (0x1 << BMI088_GYRO_INT3_GPIO_MODE_SHFITS)
// #define BMI088_GYRO_INT3_GPIO_LVL_SHFITS 0x0
// #define BMI088_GYRO_INT3_GPIO_LOW (0x0 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)
// #define BMI088_GYRO_INT3_GPIO_HIGH (0x1 << BMI088_GYRO_INT3_GPIO_LVL_SHFITS)

// #define BMI088_GYRO_INT3_INT4_IO_MAP 0x18

// #define BMI088_GYRO_DRDY_IO_OFF 0x00
// #define BMI088_GYRO_DRDY_IO_INT3 0x01
// #define BMI088_GYRO_DRDY_IO_INT4 0x80
// #define BMI088_GYRO_DRDY_IO_BOTH \
//   (BMI088_GYRO_DRDY_IO_INT3 | BMI088_GYRO_DRDY_IO_INT4)

// #define BMI088_GYRO_SELF_TEST 0x3C
// #define BMI088_GYRO_RATE_OK_SHFITS 0x4
// #define BMI088_GYRO_RATE_OK (0x1 << BMI088_GYRO_RATE_OK_SHFITS)
// #define BMI088_GYRO_BIST_FAIL_SHFITS 0x2
// #define BMI088_GYRO_BIST_FAIL (0x1 << BMI088_GYRO_BIST_FAIL_SHFITS)
// #define BMI088_GYRO_BIST_RDY_SHFITS 0x1
// #define BMI088_GYRO_BIST_RDY (0x1 << BMI088_GYRO_BIST_RDY_SHFITS)
// #define BMI088_GYRO_TRIG_BIST_SHFITS 0x0
// #define BMI088_GYRO_TRIG_BIST (0x1 << BMI088_GYRO_TRIG_BIST_SHFITS)

// static uint8_t
// write_BMI088_accel_reg_data_error[BMI088_WRITE_ACCEL_REG_NUM][3] = {
//     {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON,
//      BMI088_ACC_PWR_CTRL_ERROR},
//     {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE,
//      BMI088_ACC_PWR_CONF_ERROR},
//     {BMI088_ACC_CONF,
//      BMI088_ACC_NORMAL | BMI088_ACC_800_HZ | BMI088_ACC_CONF_MUST_Set,
//      BMI088_ACC_CONF_ERROR},
//     {BMI088_ACC_RANGE, BMI088_ACC_RANGE_6G, BMI088_ACC_RANGE_ERROR},
//     {BMI088_INT1_IO_CTRL,
//      BMI088_ACC_INT1_IO_ENABLE | BMI088_ACC_INT1_GPIO_PP |
//      BMI088_ACC_INT1_GPIO_LOW,
//      BMI088_INT1_IO_CTRL_ERROR},
//     {BMI088_INT_MAP_DATA, BMI088_ACC_INT1_DRDY_INTERRUPT,
//      BMI088_INT_MAP_DATA_ERROR}};

// static uint8_t write_BMI088_gyro_reg_data_error[BMI088_WRITE_GYRO_REG_NUM][3] =
// {{BMI088_GYRO_RANGE, BMI088_GYRO_2000, BMI088_GYRO_RANGE_ERROR},
//  {BMI088_GYRO_BANDWIDTH,
//   BMI088_GYRO_1000_116_HZ | BMI088_GYRO_BANDWIDTH_MUST_Set,
//   BMI088_GYRO_BANDWIDTH_ERROR},
//  {BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE, BMI088_GYRO_LPM1_ERROR},
//  {BMI088_GYRO_CTRL, BMI088_DRDY_ON, BMI088_GYRO_CTRL_ERROR},
//  {BMI088_GYRO_INT3_INT4_IO_CONF,
//   BMI088_GYRO_INT3_GPIO_PP | BMI088_GYRO_INT3_GPIO_LOW,
//   BMI088_GYRO_INT3_INT4_IO_CONF_ERROR},
//  {BMI088_GYRO_INT3_INT4_IO_MAP, BMI088_GYRO_DRDY_IO_INT3,
//   BMI088_GYRO_INT3_INT4_IO_MAP_ERROR}};

// static uint8_t const write_BMI088_ACCEL_self_test_Reg_Data_Error[6][3] = {
//     {BMI088_ACC_CONF,
//      BMI088_ACC_NORMAL | BMI088_ACC_1600_HZ | BMI088_ACC_CONF_MUST_Set,
//      BMI088_ACC_CONF_ERROR},
//     {BMI088_ACC_PWR_CTRL, BMI088_ACC_ENABLE_ACC_ON, BMI088_ACC_PWR_CTRL_ERROR},
//     {BMI088_ACC_RANGE, BMI088_ACC_RANGE_24G, BMI088_ACC_RANGE_ERROR},
//     {BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE_MODE,
//      BMI088_ACC_PWR_CONF_ERROR},
//     {BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_POSITIVE_SIGNAL,
//      BMI088_ACC_PWR_CONF_ERROR},
//     {BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_NEGATIVE_SIGNAL,
//      BMI088_ACC_PWR_CONF_ERROR}};

// class Bmi088 {
// public:
//   using StateEnum = enum {
//     Null = 0,
//     Connected = 1,
//     Lost = 2,
//     Error = 3,
//     Pending = 4
//   };

// public:
//   /* 测量值 */
//   AccelStruct accel_measurements{};
//   GyroStruct gyro_measurements{};
//   units::temperature::celsius_t temperature{0};
//   /* 性能观测项 */
//   units::time::microsecond_t update_dt{0};
//   float last_update_tick{0};

// private:
//   /* 基本设备 */
//   bsp::SpiDevice* _spi{nullptr};
//   bsp::GpioDriver* _accel_chip_selection_pin{nullptr};
//   bsp::GpioDriver* _gyro_chip_selection_pin{nullptr};
//   /* 偏置 */
//   GyroStruct _gyro_offset{};
//   /* 设备状态 */
//   StateEnum _state{Null};
//   /* 设备参数及常数 */
//   units::acceleration::meters_per_second_squared_t _g_norm{0};
//   units::acceleration::meters_per_second_squared_t _accel_sen{0};
//   units::angular_velocity::degrees_per_second_t _gyro_sen{0};
//   units::dimensionless::scalar_t _accel_scale{0};

// public:
//   /**
//    * @brief 依赖注入
//    * @param [SpiDevice *] spi SPI 设备
//    * @param [GpioDevice *] accel_chip_selection_pin 加速度计片选引脚
//    * @param [GpioDevice *] gyro_chip_selection_pin 陀螺仪片选引脚
//    */
//   Bmi088(bsp::SpiDevice* spi, bsp::GpioDriver* accel_chip_selection_pin,
//          bsp::GpioDriver* gyro_chip_selection_pin)
//     : _spi{spi},
//       _accel_chip_selection_pin{accel_chip_selection_pin},
//       _gyro_chip_selection_pin{gyro_chip_selection_pin} {
//     reset();
//   };

//   void getOffset() {
//     using namespace units::literals;
//     _gyro_offset.pitch = 0.00247530174_deg_per_s;
//     _gyro_offset.roll = 0.000393082853_deg_per_s;
//     _gyro_offset.yaw = 0.000393082853_deg_per_s;
//     _g_norm = 9.69293118_mps_sq;
//     _accel_scale = GRAVITY / _g_norm;
//   }

//   Bmi088* init() {
//     using namespace units::literals;
//     reset();

//     uint8_t error = BMI088_NO_ERROR;
//     do {
//       _accel_sen = units::acceleration::meters_per_second_squared_t(BMI088_ACCEL_6G_SEN);
//       _gyro_sen = units::angular_velocity::degrees_per_second_t(BMI088_GYRO_2000_SEN);

//       error |= (Bmi088AccelTest(this) != BMI088_NO_ERROR)
//                  ? BMI088_SELF_TEST_ACCEL_ERROR
//                  : Bmi088AccelInit(this);

//       error |= (Bmi088GyroTest(this) != BMI088_NO_ERROR)
//                  ? BMI088_SELF_TEST_GYRO_ERROR
//                  : Bmi088GyroInit(this);

//       getOffset();

//       _state = error != BMI088_NO_ERROR ? Lost : Connected;
//     } while (error);

//     return this;
//   };

//   Bmi088* update() {
//     using namespace bsp;
//     uint8_t buff[8] = {0, 0, 0, 0, 0, 0};
//     int16_t raw_temp;

//     _state = Pending;
//     update_dt = os::GetTime() - update_dt;
//     last_update_tick = os::GetTime()();

//     accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);

//     accel_measurements.x =
//         ((int16_t)((buff[1]) << 8) | buff[0]) * _accel_sen * _accel_scale;
//     accel_measurements.y =
//         ((int16_t)((buff[3]) << 8) | buff[2]) * _accel_sen * _accel_scale;
//     accel_measurements.z =
//         ((int16_t)((buff[5]) << 8) | buff[4]) * _accel_sen * _accel_scale;

//     gyroReadMultiReg(BMI088_GYRO_CHIP_ID, buff, 8);
//     if (buff[0] == BMI088_GYRO_CHIP_ID_VALUE) {
//       gyro_measurements.pitch =
//           ((int16_t)((buff[3]) << 8) | buff[2]) * _gyro_sen -
//           _gyro_offset.pitch;
//       gyro_measurements.roll =
//           ((int16_t)((buff[5]) << 8) | buff[4]) * _gyro_sen - _gyro_offset.roll;
//       gyro_measurements.yaw =
//           ((int16_t)((buff[7]) << 8) | buff[6]) * _gyro_sen - _gyro_offset.yaw;
//     }
//     accelReadMultiReg(BMI088_TEMP_M, buff, 2);

//     raw_temp = (int16_t)((buff[0] << 3) | (buff[1] >> 5));
//     if (raw_temp > 1023) {
//       raw_temp -= 2048;
//     }

//     temperature = units::temperature::celsius_t(raw_temp * BMI088_TEMP_FACTOR + BMI088_TEMP_OFFSET);
//     _state = Connected;
//     return this;
//   };

//   Bmi088* reset() {
//     using namespace units::literals;
//     accel_measurements.x = 0_mps_sq;
//     accel_measurements.y = 0_mps_sq;
//     accel_measurements.z = 0_mps_sq;
//     gyro_measurements.pitch = 0_deg_per_s;
//     gyro_measurements.roll = 0_deg_per_s;
//     gyro_measurements.yaw = 0_deg_per_s;
//     temperature = 0_degC;
//     update_dt = 0_ms;
//     last_update_tick = 0;
//     return this;
//   };

//   Bmi088* accelWriteSingleReg(uint8_t reg, uint8_t data) {
//     uint8_t rxData;
//     _accel_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg, &rxData)->swapOneByteData(data, &rxData);
//     _accel_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* accelReadSingleReg(uint8_t reg, uint8_t* data_ptr) {
//     uint8_t rxData;
//     _accel_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg | 0x80, &rxData)
//         ->swapOneByteData(0x55, &rxData)
//         ->swapOneByteData(0x55, data_ptr);
//     _accel_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* accelReadMultiReg(uint8_t reg, uint8_t* data_ptr, uint8_t len) {
//     _accel_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg | 0x80, data_ptr)
//         ->swapOneByteData(reg | 0x80, data_ptr)
//         ->readMultiReg(0x55, data_ptr, len);
//     _accel_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* gyroWriteSingleReg(uint8_t reg, uint8_t data) {
//     uint8_t rxData;
//     _gyro_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg, &rxData)->swapOneByteData(data, &rxData);
//     _gyro_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* gyroReadSingleReg(uint8_t reg, uint8_t* data_ptr) {
//     uint8_t rxData;
//     _gyro_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg | 0x80, &rxData)->swapOneByteData(0x55, data_ptr);
//     _gyro_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* gyroReadMultiReg(uint8_t reg, uint8_t* data_ptr, uint8_t len) {
//     _gyro_chip_selection_pin->reset();
//     _spi->swapOneByteData(reg | 0x80, data_ptr)
//         ->readMultiReg(0x55, data_ptr, len);
//     _gyro_chip_selection_pin->set();
//     return this;
//   };

//   Bmi088* setOffset() {
//     using namespace bsp;
//     using namespace units::literals;

//     static uint16_t cali_times = 6000;
//     units::angular_velocity::degrees_per_second_t gyro_max[3];
//     units::angular_velocity::degrees_per_second_t gyro_min[3];
//     units::acceleration::meters_per_second_squared_t g_norm_max;
//     units::acceleration::meters_per_second_squared_t g_norm_min;
//     units::acceleration::meters_per_second_squared_t g_norm_diff = g_norm_max - g_norm_min;
//     units::angular_velocity::degrees_per_second_t gyro_diff[3];
//     uint8_t buff[8] = {0, 0, 0, 0, 0, 0};
//     int16_t cali_count = 0;
//     units::time::second_t start_time = os::GetDWTTime();

//     _gyro_offset.pitch = units::angular_velocity::degrees_per_second_t(BMI088_GxOFFSET);
//     _gyro_offset.roll = units::angular_velocity::degrees_per_second_t(BMI088_GyOFFSET);
//     _gyro_offset.yaw = units::angular_velocity::degrees_per_second_t(BMI088_GzOFFSET);
//     _g_norm = units::acceleration::meters_per_second_squared_t(BMI088_gNORM);

//     do {
//       if (os::GetDWTTime() - start_time > 10_s) {
//         _gyro_offset.pitch = units::angular_velocity::degrees_per_second_t(BMI088_GxOFFSET);
//         _gyro_offset.roll = units::angular_velocity::degrees_per_second_t(BMI088_GyOFFSET);
//         _gyro_offset.yaw = units::angular_velocity::degrees_per_second_t(BMI088_GzOFFSET);
//         _g_norm = units::acceleration::meters_per_second_squared_t(BMI088_gNORM);
//         break;
//       }
//       os::Sleep(5_ms);
//       _gyro_offset.pitch = 0_deg_per_s;
//       _gyro_offset.roll = 0_deg_per_s;
//       _gyro_offset.yaw = 0_deg_per_s;
//       _g_norm = 0_mps_sq;

//       for (uint16_t i = 0; i < cali_times; i++) {
//         accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);
//         accel_measurements.x =
//             (static_cast<int16_t>((buff[1]) << 8) | buff[0]) * _accel_sen;
//         accel_measurements.y =
//             (static_cast<int16_t>((buff[3]) << 8) | buff[2]) * _accel_sen;
//         accel_measurements.z =
//             (static_cast<int16_t>((buff[5]) << 8) | buff[4]) * _accel_sen;

//         units::acceleration::meters_per_second_squared_t g_norm_temp = units::math::sqrt(
//             accel_measurements.x * accel_measurements.x +
//             accel_measurements.y * accel_measurements.y +
//             accel_measurements.z * accel_measurements.z);
//         _g_norm += g_norm_temp;

//         gyroReadMultiReg(BMI088_GYRO_CHIP_ID, buff, 8);
//         if (buff[0] == BMI088_GYRO_CHIP_ID_VALUE) {
//           gyro_measurements.pitch =
//               (static_cast<int16_t>((buff[3]) << 8) | buff[2]) * _gyro_sen;
//           gyro_measurements.roll =
//               (static_cast<int16_t>((buff[5]) << 8) | buff[4]) * _gyro_sen;
//           gyro_measurements.yaw =
//               (static_cast<int16_t>((buff[7]) << 8) | buff[6]) * _gyro_sen;
//           _gyro_offset.pitch += gyro_measurements.pitch;
//           _gyro_offset.roll += gyro_measurements.roll;
//           _gyro_offset.yaw += gyro_measurements.yaw;
//         }

//         if (i == 0) {
//           g_norm_max = g_norm_temp;
//           g_norm_min = g_norm_temp;
//           gyro_max[0] = gyro_measurements.pitch;
//           gyro_min[0] = gyro_measurements.pitch;
//           gyro_max[1] = gyro_measurements.roll;
//           gyro_min[1] = gyro_measurements.roll;
//           gyro_max[2] = gyro_measurements.yaw;
//           gyro_min[2] = gyro_measurements.yaw;
//         } else {
//           GetMaxandMinRange(g_norm_temp, g_norm_max, g_norm_min);
//           GetMaxandMinRange(gyro_measurements.pitch, gyro_max[0], gyro_min[0]);
//           GetMaxandMinRange(gyro_measurements.roll, gyro_max[1], gyro_min[1]);
//           GetMaxandMinRange(gyro_measurements.yaw, gyro_max[2], gyro_min[2]);
//         }

//         for (uint8_t j = 0; j < 3; j++) {
//           gyro_diff[j] = gyro_max[j] - gyro_min[j];
//         }
//         if (g_norm_diff > 0.7_mps_sq || gyro_diff[0] > 0.15_deg_per_s ||
//             gyro_diff[1] > 0.15_deg_per_s || gyro_diff[2] > 0.15_deg_per_s) {
//           break;
//         }
//         os::Sleep(5_ms);
//       }

//       _g_norm /= static_cast<float>(cali_times);

//       _gyro_offset.pitch /= cali_times;
//       _gyro_offset.roll /= cali_times;
//       _gyro_offset.yaw /= cali_times;

//       cali_count++;
//     } while (g_norm_diff > 0.7_mps_sq ||
//              units::math::fabs(_g_norm - GRAVITY) > 0.5_mps_sq ||
//              gyro_diff[0] > 0.15_deg_per_s || gyro_diff[1] > 0.15_deg_per_s ||
//              gyro_diff[2] > 0.15_deg_per_s ||
//              units::math::fabs(_gyro_offset.pitch) > 0.01_deg_per_s ||
//              units::math::fabs(_gyro_offset.roll) > 0.01_deg_per_s ||
//              units::math::fabs(_gyro_offset.yaw) > 0.01_deg_per_s);

//     _accel_scale = GRAVITY / _g_norm;
//     return this;
//   };

//   friend uint8_t Bmi088AccelInit(Bmi088* bmi088) {
//     uint8_t res = 0;

//     // check commiunication
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // accel software reset
//     bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET,
//                                 BMI088_ACC_SOFTRESET_VALUE);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//     // check commiunication is normal after reset
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // check the "who am I"
//     if (res != BMI088_ACC_CHIP_ID_VALUE) {
//       return BMI088_NO_SENSOR;
//     }

//     // set accel sonsor config and check
//     for (auto& write_reg_num : write_BMI088_accel_reg_data_error) {
//       bmi088->accelWriteSingleReg(write_reg_num[0], write_reg_num[1]);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       bmi088->accelReadSingleReg(write_reg_num[0], &res);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       if (res != write_reg_num[1]) {
//         return write_reg_num[2];
//       }
//     }
//     return BMI088_NO_ERROR;
//   };

//   friend uint8_t Bmi088GyroInit(Bmi088* bmi088) {
//     uint8_t res = 0;

//     // check commiunication
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // reset the gyro sensor
//     bmi088->gyroWriteSingleReg(BMI088_GYRO_SOFTRESET,
//                                BMI088_GYRO_SOFTRESET_VALUE);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);
//     // check commiunication is normal after reset
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // check the "who am I"
//     if (res != BMI088_GYRO_CHIP_ID_VALUE) {
//       return BMI088_NO_SENSOR;
//     }

//     // set gyro sonsor config and check
//     for (auto& write_reg_num : write_BMI088_gyro_reg_data_error) {
//       bmi088->gyroWriteSingleReg(write_reg_num[0], write_reg_num[1]);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       bmi088->gyroReadSingleReg(write_reg_num[0], &res);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       if (res != write_reg_num[1]) {
//         return write_reg_num[2];
//       }
//     }

//     return BMI088_NO_ERROR;
//   };

//   friend uint8_t Bmi088AccelTest(Bmi088* bmi088) {
//     uint8_t res = 0;
//     int16_t self_test_accel[2][3];
//     uint8_t buff[6] = {0, 0, 0, 0, 0, 0};

//     // check commiunication is normal
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // reset  bmi088 accel sensor and wait for > 50ms
//     bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET,
//                                 BMI088_ACC_SOFTRESET_VALUE);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//     // check commiunication is normal
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     if (res != BMI088_ACC_CHIP_ID_VALUE) {
//       return BMI088_NO_SENSOR;
//     }

//     // set the accel register
//     for (int write_reg_num = 0; write_reg_num < 4; write_reg_num++) {
//       bmi088->accelWriteSingleReg(
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][0],
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][1]);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       bmi088->accelReadSingleReg(
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][0], &res);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       if (res !=
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][1]) {
//         return write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num][2];
//       }

//       // accel conf and accel range  . the two register set need wait for > 50ms
//       bsp::Delay_ms(BMI088_LONG_DELAY_TIME);
//     }

//     // self test include postive and negative
//     for (int write_reg_num = 0; write_reg_num < 2; write_reg_num++) {
//       bmi088->accelWriteSingleReg(
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][0],
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][1]);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       bmi088->accelReadSingleReg(
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][0],
//           &res);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//       if (res !=
//           write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4][1]) {
//         return write_BMI088_ACCEL_self_test_Reg_Data_Error[write_reg_num + 4]
//             [2];
//       }

//       // accel conf and accel range  . the two register set need wait for > 50ms
//       bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//       // read response accel
//       bmi088->accelReadMultiReg(BMI088_ACCEL_XOUT_L, buff, 6);
//       self_test_accel[write_reg_num][0] =
//           static_cast<int16_t>((buff[1] << 8) | buff[0]);
//       self_test_accel[write_reg_num][1] =
//           static_cast<int16_t>((buff[3] << 8) | buff[2]);
//       self_test_accel[write_reg_num][2] =
//           static_cast<int16_t>((buff[5] << 8) | buff[4]);
//     }

//     // set self test off
//     bmi088->accelWriteSingleReg(BMI088_ACC_SELF_TEST, BMI088_ACC_SELF_TEST_OFF);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_SELF_TEST, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     if (res != (BMI088_ACC_SELF_TEST_OFF)) {
//       return BMI088_ACC_SELF_TEST_ERROR;
//     }

//     // reset the accel sensor
//     bmi088->accelWriteSingleReg(BMI088_ACC_SOFTRESET,
//                                 BMI088_ACC_SOFTRESET_VALUE);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//     if ((self_test_accel[0][0] - self_test_accel[1][0] < 1365) ||
//         (self_test_accel[0][1] - self_test_accel[1][1] < 1365) ||
//         (self_test_accel[0][2] - self_test_accel[1][2] < 680)) {
//       return BMI088_SELF_TEST_ACCEL_ERROR;
//     }

//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->accelReadSingleReg(BMI088_ACC_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     return BMI088_NO_ERROR;
//   };

//   friend uint8_t Bmi088GyroTest(Bmi088* bmi088) {
//     uint8_t res = 0;
//     uint8_t retry = 0;

//     // check commiunication is normal
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     // reset the gyro sensor
//     bmi088->gyroWriteSingleReg(BMI088_GYRO_SOFTRESET,
//                                BMI088_GYRO_SOFTRESET_VALUE);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//     // check commiunication is normal after reset
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//     bmi088->gyroReadSingleReg(BMI088_GYRO_CHIP_ID, &res);
//     bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);

//     bmi088->gyroWriteSingleReg(BMI088_GYRO_SELF_TEST, BMI088_GYRO_TRIG_BIST);
//     bsp::Delay_ms(BMI088_LONG_DELAY_TIME);

//     do {
//       bmi088->gyroReadSingleReg(BMI088_GYRO_SELF_TEST, &res);
//       bsp::Delay_ms(BMI088_COM_WAIT_SENSOR_TIME);
//       retry++;
//     } while (!(res & BMI088_GYRO_BIST_RDY) && retry < 10);

//     if (retry == 10) {
//       return BMI088_SELF_TEST_GYRO_ERROR;
//     }

//     if (res & BMI088_GYRO_BIST_FAIL) {
//       return BMI088_SELF_TEST_GYRO_ERROR;
//     }

//     return BMI088_NO_ERROR;
//   };
// };

// namespace bsp {
// namespace bmi088 {
// struct InsDataType {
//   units::angle::degree_t roll;
//   units::angle::degree_t pitch;
//   units::angle::degree_t yaw;
//   units::angle::degree_t yaw_total_angle;
//   units::dimensionless::scalar_t yaw_round;
//   units::angle::degree_t yaw_zero_drift;
//   units::angle::degree_t yaw_offset;
//   units::angular_velocity::degrees_per_second_t roll_speed;
//   units::angular_velocity::degrees_per_second_t pitch_speed;
//   units::angular_velocity::degrees_per_second_t yaw_speed;
//   void set_yaw_offset(units::angle::degree_t angle) { yaw_offset = angle; }
// };

// InsDataType const& GetImuDataRef();
// void Start();
// void Update();
// } // namespace bmi088
// } // namespace bsp